#include "ros/ros.h"
#include "cyberarm_controller/property_msg.h"
#include "cyberarm_controller/me_msg.h"
#include <sstream>
#include "get_state.hpp"

#define MOTOR_NUM 4
#define MOTOR_ID 4 // 根据电机ID号进行修改

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ROS_INFO("arm_set_zero");
    ros::init(argc, argv, "motor_set_zero_node");
    ros::NodeHandle nh;
    ros::Publisher write_property_pub = nh.advertise<cyberarm_controller::property_msg>("write_property", 10); // 属性修改
    ros::Subscriber sub = nh.subscribe("arm_state", 1000, GetState);
    ros::Rate rate_delay(1); // 延时1s等待CAN接口初始化之后再进行reply_state配置，否则可能会出现配置不成功的情况
    rate_delay.sleep();
    double pos_err_thre = 1e-1;
    ros::Rate step(20);
    // bool flag = true;
    // while(flag == true)
    // {
        // nh.getParam("zero_mode_allow", flag);
    int times = 0;
    while(ros::ok() && times < 10)
    {
        times++;
        for(unsigned int i = 0; i < MOTOR_NUM; i++)
        {
            cyberarm_controller::property_msg p_message;
            p_message.id = i + 1;
            p_message.cmd_mode = 6;
            p_message.address = 0x7001; // run_mode
            p_message.data_type = 5; // 数据类型 u8
            p_message.value = 0; // mode = 3:  扭矩模式
            write_property_pub.publish(p_message);
            step.sleep();
        }
    }
    
    
    return 0;
}
